﻿using Newtonsoft.Json;
using System;
using System.Net;
using System.Net.Http;
using System.Net.Sockets;
using System.Threading;

namespace GalileoAPIDemo
{

    public class SystemStatus
    {
        public int camera_rgb;
        public int battery;
        public int camera_processed;
        public int imu;
        public int odom;
        public int camera_depth;
        public bool driver_port = false;
        public bool charge = false;
        public bool loop = false;
        public ServerInfoSimple info = null;
        public string slam_type = "camera";
    }

    public class ServerInfoResponse
    {
        public ServerInfoSimple info;
    }

    public class ServerInfoSimple
    {
        public string version;
        public string code_name;
        public string id;
        public string mac;
        public int port;
    }

    public class Pose
    {
        public double x;
        public double y;
        public double angle;
    }

    public class TargetPose
    {
        public double x;
        public double y;
        public double theta;
    }

    public class ServerInfo
    {
        public string ip;
        public int port;
        public long timestamp;
        public string id;
        public string password = "xiaoqiang";
        public string mac;
        public int udpPort;
        public string version;
    }

    public class TokenResponse
    {
        public string result;
        public string token;
    }

    public class SetSpeedReq
    {
        public float speed_x;
        public float speed_y;
        public float speed_angle;
    }

    public class TaskInfo
    {
        public string id;
        public string state;
    }

    class Program
    {
        static void Main(string[] args)
        {
            /*
             * 整个程序包含以下几个过程。
             * 获取局域网机器人广播，然后获取机器人Token。
             * 获取机器人当前基本信息，获取机器人当前导航坐标，然后发布导航任务，机器人移动到前方0.5m处，并等待任务完成。
             * 使用速度控制API，让机器人原地旋转1秒。
             * 重置机器人遥控速度参数
             * 这几个API分别包含了几种API的请求方式，GET, POST, PUT, DELETE。其他API调用和这几个例子一样。
             */

            // 获取机器人UDP广播
            UdpClient serverSocket = new UdpClient();
            serverSocket.Client.ReceiveTimeout = 1000;
            serverSocket.Client.Bind(new IPEndPoint(IPAddress.Any, 22002));
            serverSocket.Client.ReceiveTimeout = 60;
            ServerInfo serverInfo;
            IPEndPoint BroadServEP = new IPEndPoint(IPAddress.Any, 0);
            Console.WriteLine("Waitting UDP broadcast data");
            try
            {
                var receiveData = serverSocket.Receive(ref BroadServEP);
                string receivedString = System.Text.Encoding.UTF8.GetString(receiveData);
                serverInfo = JsonConvert.DeserializeObject<ServerInfo>(receivedString);
                Console.WriteLine("Received UDP broadcast data");
                Console.WriteLine(receivedString);
                serverInfo.ip = BroadServEP.Address.ToString();
                serverSocket.Close();
            }
            catch (Exception)
            {
                Console.WriteLine("Get brocast data failed");
                return;
            }

            string robotIP = serverInfo.ip;
            string apiURL = $"http://{robotIP}:3546/api/v1";
            // 获取机器人token，采用默认的用户名密码
            TokenResponse token;
            using (var client = new HttpClient())
            {
                var resStr = client.GetStringAsync($"{apiURL}/token?username=admin&password=admin").GetAwaiter().GetResult();
                token = JsonConvert.DeserializeObject<TokenResponse>(resStr);
                Console.WriteLine("Get robot token");
            }
            Console.WriteLine("Start get robot info");
            // 获取机器人基本信息
            using (var client = new HttpClient())
            {
                var resStr = client.GetStringAsync($"{apiURL}/system/info?token={token.token}").GetAwaiter().GetResult();
                var resJson = JsonConvert.DeserializeObject<SystemStatus>(resStr);
                // 打印当前电压
                Console.WriteLine($"Current battery: {resJson.battery}");
            }
            Thread.Sleep(3000);
            // 执行前先开启机器人导航
            // 获取当前坐标
            Console.WriteLine("Start get robot pose");
            Pose currentPose;
            using (var client = new HttpClient())
            {
                var resStr = client.GetStringAsync($"{apiURL}/navigation/pose?token={token.token}").GetAwaiter().GetResult();
                var resJson = JsonConvert.DeserializeObject<Pose>(resStr);
                // 打印当前坐标
                currentPose = resJson;
                Console.WriteLine($"Current pose: x {resJson.x} y {resJson.y} angle {resJson.angle}");
            }
            Thread.Sleep(3000);
            Console.WriteLine("Start move to target point");
            // 移动到目标,向前移动0.5m
            double x = currentPose.x + 0.5 * Math.Cos(currentPose.angle);
            double y = currentPose.y + 0.5 * Math.Sin(currentPose.angle);
            TargetPose pose = new TargetPose
            {
                x = x,
                y = y,
                theta = currentPose.angle
            };
            using (var client = new HttpClient())
            {
                var res = client.PostAsync($"{apiURL}/navigation/start_nav_task?token={token.token}",
                    new StringContent(JsonConvert.SerializeObject(pose))).GetAwaiter().GetResult();
                if (res.IsSuccessStatusCode)
                    Console.WriteLine("send navigation task succeed");
                else
                {
                    Console.WriteLine("send navigation task failed");
                    return;
                }
                TaskInfo taskInfo = JsonConvert.DeserializeObject<TaskInfo>(res.Content.ReadAsStringAsync().GetAwaiter().GetResult());
                // 通过轮询等待任务执行完成
                while (true)
                {
                    var res1 = client.GetStringAsync($"{apiURL}/task?id={taskInfo.id}&token={token.token}").GetAwaiter().GetResult();
                    TaskInfo info = JsonConvert.DeserializeObject<TaskInfo>(res1);
                    if (info.state == "COMPLETE" || info.state == "CANCELLED" || info.state == "ERROR")
                        break;
                    Console.WriteLine("Waitting for task complete...");
                    Thread.Sleep(1000);
                }
            }
            // 让机器人原地旋转1s
            using (var client = new HttpClient())
            {
                var res = client.PutAsync($"{apiURL}/system/speed?token={token.token}",
                    new StringContent(JsonConvert.SerializeObject(
                        new SetSpeedReq
                        {
                            speed_x = 0, // 机器人向前的速度
                            speed_y = 0, // 机器人向左平移速度，仅全向轮可用
                            speed_angle = 1.0f // 机器人旋转角速度
                        }
                ))).GetAwaiter().GetResult();
                Thread.Sleep(1000);
                // 停止旋转
                res = client.PutAsync($"{apiURL}/system/speed?token={token.token}",
                    new StringContent(JsonConvert.SerializeObject(
                        new SetSpeedReq
                        {
                            speed_x = 0, // 机器人向前的速度
                            speed_y = 0, // 机器人向左平移速度，仅全向轮可用
                            speed_angle = 0.0f // 机器人旋转角速度
                        }
                ))).GetAwaiter().GetResult();
            }
            // 重置机器人最大遥控速度 max_control_speed
            using (var client = new HttpClient())
            {
                var res = client.DeleteAsync($"{apiURL}/system/config?token={token.token}&key=max_control_speed")
                    .GetAwaiter().GetResult();
                if (res.IsSuccessStatusCode)
                    Console.WriteLine("Reset max control speed success");
                else
                    Console.WriteLine("Reset max control speed failed");
            }
        }
    }
}
